import java.lang.*;
import java.awt.*;
import java.util.*;

public class HuristicAStar extends OneTailAStar implements PathFinder
{
	double minCost;

	public HuristicAStar(){
		super();
		stepSpeed=20;
	}
	/**
	 * calculates the waighted manhattan distance from a to b
	 */
	/*public double cbDist(Point a,Point b,double low){
        return low * (Math.abs(a.x-b.x)+Math.abs(a.y-b.y)-1);
    }*/

	public double digDist(Point a,Point b,double low){//Add diagonal distance
		return low * Math.max(Math.abs(a.x-b.x),Math.abs(a.y-b.y));
	}


	public GridCell[] findPath(Map map,int [] pushBgt)
	{

		minCost = Double.MAX_VALUE;
		for(int i=0;i<map.w;i++){
			for(int j=0;j<map.h;j++){
				minCost = Math.min(map.gridCell[j][i].getCost(),minCost);
			}
		}
		//minCost=0.9;
		System.out.println("Cheepest Tile = "+minCost);
		return super.findPath(map,pushBgt);
	}

	public int step(int [] psBgt){
		int tests = 0;
		boolean found = false;
		boolean growth = true;
		GridCell finish = GridCell.getFinishCell();
		Point end = finish.getPosition();
		ArrayList temp = (ArrayList) edge.clone();


		//System.out.println("psBgtAFstep:"+psBgt);
		//this.showPushes(psBgt,"push BGT passed to step");

		System.out.println("EDGE:");
		if (edge.size()!=0){
			for (int i=0;i<edge.size();i++)
			{
				GridCell g=(GridCell) edge.get(i);
				System.out.println(g.getPosition());
			}
			System.out.println();
		} else System.out.println("EMPTY");

		System.out.println("DONE:");
		if (done.size()!=0){
			for (int i=0;i<done.size();i++)
			{
				GridCell g=(GridCell) done.get(i);
				System.out.println(g.getPosition());
			}
			System.out.println();
		}else System.out.println("EMPTY");


		//find the most promesing edge cell
		double min = Double.MAX_VALUE;
		double score;
		//int best = -1;
		GridCell best = null;// = (GridCell)temp.get(temp.size()-1); 
		GridCell now;

		for(int i=0;i<temp.size();i++){
			now = (GridCell)temp.get(i);

			if(!done.contains(now)){
				score =now.getDistFromStart();

				score += digDist(now.getPosition(),end,minCost);
				System.out.println("F["+i+"]="+score);
				if(score<=min){
					min = score;
					best = now;
				}
			}
		}

		now = best;
		System.out.println("now:"+now.getPosition());


		if(now.getCameFrom()!=null)
			now.getVel().newVelocity(now.getCameFrom(),now.getPsNd());

		System.out.println("nowVelo:"+now.getVel());

		int[] psNd=now.getPsNd();

		//System.out.println("psNd:"+psNd+"psBgt:"+psBgt);


		int [] currentPsBgt=psBgt.clone();
		GridCell tk=now;


		while (tk.getCameFrom()!=null){
			System.out.println("TK:"+tk.getPosition());
			this.showPushes(tk.getPsNd(), "TK.PsNd:");
			consumePush(tk.getPsNd(),currentPsBgt);
			tk=tk.getCameFrom();			
		}


		//if(psNd!=null)
		//consumePush(psNd,psBgt);

		//System.out.println(now.getPosition()+" Selected for expansion");
		edge.remove(now);
		System.out.println("Cell:"+now.getPosition()+"Remove Edge");
		done.add(now);
		System.out.println("Cell:"+now.getPosition()+"Add Done");
		
		
		
		
		GridCell next[] = map.getAdjacent(now);
		boolean ifRelease = true;
		for(int i=0;i<9;i++){//extend to 9 neighbors
			if(next[i]!=null){
				if(!next[i].isTotalBlock()){
					//Cannot go across corner.
					if (i==4&&(next[0].isTotalBlock()||next[1].isTotalBlock())) continue;
					if (i==5&&(next[1].isTotalBlock()||next[2].isTotalBlock())) continue;
					if (i==6&&(next[2].isTotalBlock()||next[3].isTotalBlock())) continue;
					if (i==7&&(next[3].isTotalBlock()||next[0].isTotalBlock())) continue;
					//Cannot reach constrained by physical law.
					System.out.println("Next["+i+"]");
					GridCell gc=new GridCell();
					gc.setPosition(next[i].getPosition());
					int[] pushesNeeded=needPushes(now,next[i]);
					if (inBudget(pushesNeeded,currentPsBgt)){
						if(gc.getPosition().equals(finish.getPosition())){found=true; this.goalNode=gc;} else{
							
							
							if(!edge.contains(gc) && !done.contains(gc)){
								System.out.println("Cell:"+gc.getPosition()+"Add to Edge");
								//edge.add(next[i]);growth=true;
								edge.add(gc);growth=true;
							}
						}
						if (edge.contains(next[i])) ifRelease=false;
						tests++;
						//next[i].addToPathFromStart(now,pushesNeeded);//Calculate g value of cells in open list and unexamined.
						gc.addToPathFromStart(now,pushesNeeded);
						next[i].setDisFromStart(gc.getDistFromStart());
						
					}

				}
			}
			if(found){return FOUND;}
		}
		map.repaint();
		if(edge.size()==0){return NO_PATH;}

		//now process best.

		if((psNd!=null)&&ifRelease){			
			//this.releasePush(psNd, currentPsBgt);
			//edge.add(now.getParentCell());
			//System.out.println("Cell:"+now.getPosition()+"Add to Edge");
			//done.remove(now.getParentCell());
			//System.out.println("Cell:"+now.getPosition()+"Add Done");
		}

		return NOT_FOUND;
	}


	private void consumePush(int[] psNd, int[] psBgt) {
		for(int i=0;i<4;i++){
			psBgt[i]-=psNd[i];
		}
	}

	/*private void releasePush(int[] psNd, int[] psBgt) {
		for(int i=0;i<4;i++){
			psBgt[i]+=psNd[i];
		}
	}*/

	private boolean inBudget(int [] psNeed, int [] psSupply) {
		this.showPushes(psNeed, "psNeed");
		this.showPushes(psSupply, "psSupply");
		for(int i=0;i<4;i++){
			if (psNeed[i]<=psSupply[i]) continue;	
			else{
				System.out.println("Out BGT!");
				return false;	
			}

		}
		System.out.println("In BGT!");		
		return true;


	}

	private int[] needPushes(GridCell now, GridCell next) {

		int[] pushVector={0,0,0,0};
		//ArrayList v=new ArrayList();		
		int nowPositionX=(int) now.getPosition().getX();
		int nowPositionY=(int) now.getPosition().getY();
		int nextPositionX=(int) next.getPosition().getX();
		int nextPositionY=(int) next.getPosition().getY();

		Push delta = new Push(nextPositionX-nowPositionX-now.getVel().getXvel(),
				nextPositionY-nowPositionY-now.getVel().getYvel());
		System.out.println("Delta:"+delta.toString());
		//Push deltaX=new Push(delta.getXpush(),0);
		int xPush=delta.getXpush();
		int yPush=delta.getYpush();

		if (xPush>0) pushVector[0]=xPush;//To Right 
		if (xPush<0) pushVector[1]=Math.abs(xPush);//To Left

		if (yPush>0) pushVector[2]=yPush;//To Down 
		if (yPush<0) pushVector[3]=Math.abs(yPush);//To Up


		return pushVector;
	}

	private void showPushes(int[] v, String n){
		System.out.print(n+":");	
		for(int j=0;j<v.length;j++){
			System.out.print(v[j]+" ");			
		}
		System.out.println();

	}
}